#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/static_transform_broadcaster.hpp>
// #include <tf2_ros/static_transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.hpp>
class TFStaticBroadcastNode : public rclcpp::Node
{
private:
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcast_;

public:
    TFStaticBroadcastNode(char *argv[]) : Node("TF_static_broadcast_node_cpp")
    {
        RCLCPP_INFO(this->get_logger(), "TFStaticBroadcastNodeCpp创建");
        //
        static_broadcast_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
        // 组织发布数据
        this->pub_static_tf(argv);
    }

private:
    void pub_static_tf(char *argv[])
    {
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = this->now();
        transform.header.frame_id = argv[7];
        //
        transform.child_frame_id = argv[8];
        //
        transform.transform.translation.x = atof(argv[1]);
        transform.transform.translation.y = atof(argv[2]);
        transform.transform.translation.z = atof(argv[3]);
        //

        tf2::Quaternion qtn;
        qtn.setRPY(atof(argv[4]), atof(argv[5]), atof(argv[6]));
        //
        transform.transform.rotation.x = qtn.getX();
        transform.transform.rotation.y = qtn.y();
        transform.transform.rotation.z = qtn.getZ();
        transform.transform.rotation.w = qtn.getW();
        this->static_broadcast_->sendTransform(transform);
    }
};
int main(int argc, char **argv)
{
    // ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcast
    // 1 2 3 1 2 1 base_link  laser
    // x y z roll pitch yaw
    if (argc != 9)
    {
        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "请传入9个参数");
        return 1;
    }
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<TFStaticBroadcastNode>(argv));
    rclcpp::shutdown();
    return 0;
}